#include "imu.h"

//IMU931_t IMU931;
uint8_t origin_data;
double yaw_angle; 
double z_gyro;
int yaw_angle_int;  
struct Angle YawAngle;
struct Angle GYRO_Z;
uint8_t imu_angle_flag;
uint8_t imu_gyro_flag;

//static void IMU_ACC_hook(uint32_t *imu);
//static void IMU_ANGLE_hook(uint32_t *imu);
//static void IMU_Quaternion_hook(uint32_t *imu);
//static void IMU_GYRO_hook(uint32_t *imu);
//IMU_ACC_hook,
//,IMU_Quaternion_hook
////void * IMU_hook_fun[WIT_DATA_NUM] = {IMU_ANGLE_hook,IMU_GYRO_hook};

//void IMU_Init()
//{
////	unsigned char i;
////	for(i = 0 ; i < WIT_DATA_NUM ; i++)
////	{
////		imu->hook_fun[i].imu_hook = (void(*)(uint32_t *imu))IMU_hook_fun[i];
////	}
//}

void IMU_uart_callback()
{	
	static unsigned char imu_rx_buffer[250];
	static uint8_t cnt;
	imu_rx_buffer[cnt++]=origin_data;
	//数据头不对，则重新开始寻找0x55数据头
	if (imu_rx_buffer[0]!=WIT_HEADER) 
	{
		cnt=0;
		return;
	}
	//数据不满11个，则返回
	if (cnt<WIT_DATA_SUM) {return;}
	else
	{
//		if(imu->rx_buffer[1] == WIT_ACC_HEADER)	
//		{
//			imu->hook_signal = WIT_ACC ;
//			memcpy(imu->temp[WIT_ACC],imu->rx_buffer,11);
//		}
		 if(imu_rx_buffer[1] == WIT_ANGLE_HEADER) 
		{
//			imu->hook_signal = WIT_ANGLE ;	
			memcpy(&YawAngle,&imu_rx_buffer[2],8);
			imu_angle_flag=1;
//			memcpy(imu->temp[WIT_ANGLE],imu->rx_buffer,11);
		}
//		else if(imu->rx_buffer[1] == WIT_Quaternion_HEADER)
//		{
//			imu->hook_signal = WIT_Quaternion ;
//			memcpy(imu->temp[WIT_Quaternion],imu->rx_buffer,11);
//		}
		else if(imu_rx_buffer[1] == WIT_GYRO_HEADER)
		{
//			imu->hook_signal = WIT_GYRO;
//			memcpy(imu->temp[WIT_GYRO],imu->rx_buffer,11);
				memcpy(&GYRO_Z,&imu_rx_buffer[2],8);
				imu_gyro_flag = 1;
			
//				imu->data.GYRO_Z = (double)imu->rx_buffer[2+1]/32768*2000;
		}
		//清空缓存区
		cnt=0;
	}  
}
void imu_gyro_analysis()
{
	z_gyro =(float)GYRO_Z.Angle[2]/32768*2000;
}

void imu_yaw_analysis()
{
	yaw_angle = (float)YawAngle.Angle[2]/32768*180; 
	yaw_angle_int = (int)yaw_angle;
}

//void IMU_hook(IMU931_t *imu)
//{
////		if(imu->hook_signal == WIT_ACC)
////		{
////			imu->hook_fun[WIT_ACC].imu_hook((uint32_t*)imu);
////			imu->hook_signal = WIT_DATA_NUM;
////		}
//		if(imu->hook_signal == WIT_ANGLE)
//		{
//			imu->data.roll  = (double)imu->Angle.data[0]/32768*180;
//			imu->data.pitch = (double)imu->Angle.data[1]/32768*180;
//			imu->data.yaw   = (double)imu->Angle.data[2]/32768*180;	
//			imu->data.yaw_int   = (int)	imu->data.yaw;
//			imu->hook_signal = WIT_DATA_NUM;
//		}
////		else if(imu->hook_signal == WIT_Quaternion)
////		{
////			imu->hook_fun[WIT_Quaternion].imu_hook((uint32_t*)imu);
////			imu->hook_signal = WIT_DATA_NUM;
////		}
//		else if(imu->hook_signal == WIT_GYRO)
//		{
//			imu->hook_fun[WIT_GYRO].imu_hook((uint32_t*)imu);
//			imu->hook_signal = WIT_DATA_NUM;
//		}
//}

//static void IMU_ACC_hook(uint32_t *imu)
//{
//	IMU931_t *local_imu = (IMU931_t  *)imu;
//	memcpy(&local_imu->ACC.data,&local_imu->temp[WIT_ACC][2],8);
//	local_imu->data.ACC_x  = (double)local_imu->ACC.data[0]/32768*16*Gg;
//	local_imu->data.ACC_y  = (double)local_imu->ACC.data[1]/32768*16*Gg;
//	local_imu->data.ACC_z  = (double)local_imu->ACC.data[2]/32768*16*Gg;
//}

//static void IMU_ANGLE_hook(uint32_t *imu)
//{
//	IMU931_t *local_imu = (IMU931_t  *)imu;
//	memcpy(&local_imu->Angle.data,&local_imu->temp[WIT_ANGLE][2],8);
//	local_imu->data.roll  = (double)local_imu->Angle.data[0]/32768*180;
//	local_imu->data.pitch = (double)local_imu->Angle.data[1]/32768*180;
//	local_imu->data.yaw   = (double)local_imu->Angle.data[2]/32768*180;	
//	local_imu->data.yaw_int   = (int)	local_imu->data.yaw;
//}
//static void IMU_GYRO_hook(uint32_t *imu)
//{
//	IMU931_t *local_imu = (IMU931_t *)imu;
//	memcpy(&local_imu->GYRO.data,&local_imu->temp[WIT_GYRO][2],8);
//	local_imu->data.GYRO_X = (double)local_imu->GYRO.data[0]/32768*2000;
//	local_imu->data.GYRO_Y = (double)local_imu->GYRO.data[1]/32768*2000;
//	local_imu->data.GYRO_Z = (double)local_imu->GYRO.data[2]/32768*2000;
//}
//	

////static void IMU_Quaternion_hook(uint32_t *imu)
////{
////	IMU931_t *local_imu = (IMU931_t  *)imu;
////	memcpy(&local_imu->Quaternion.data,&local_imu->temp[WIT_Quaternion][2],12);
////	local_imu->data.q0  = (double)local_imu->Quaternion.data[0]/32768;
////	local_imu->data.q1  = (double)local_imu->Quaternion.data[1]/32768;
////	local_imu->data.q2  = (double)local_imu->Quaternion.data[2]/32768;
////	local_imu->data.q3  = (double)local_imu->Quaternion.data[3]/32768;	
////}


//IMU_data_t * Get_IMU_Data(IMU931_t *imu)
//{
//	return &imu->data;
//}
